%% Legal states
i = 0;
%% InitialState = First iteration of the loop. Configuration Data.
InitialState = i;
i = i + 1;
%% MapSurroundings = Check the surrounding and area. Complement representation.
MapSurroundings = i;
i = i + 1;
%% DecideAndOrient = Determine which direction is obstacle free and
% rotate the robot toward it if in exploration mode. If in return mode, the
% robot will face the next breadcrumb to be reached.
DecideAndOrient = i;
i = i + 1;
%% FragmentMove = Move a fraction of total movement.
FragmentMove = i;
i = i + 1;
%% FragmentICP = After movement remap location.
FragmentICP = i;
i = i + 1;
%% DropBreadCrumbExplore = After reloc, place a movement node for reverse
% navigation.
DropBreadCrumbExplore = i;
i = i + 1;
%% MovementCompletedExplore = Once at destination during exploration, Check
% if mode changed (exploration?). If not, return to MapSurroundings, if changed
% go to return mode
MovementCompletedExplore = i;
i = i + 1;
%% InitReturnToBaseMode = Rotate the robot toward the previous breadcrumb.
% Reloc using ICP. Changed to FragmentMove
InitReturnToBaseMode = i;
i = i + 1;
%% MovementCompletedReturnMode = Checks if at origin. If not, rotate toward
% previous node and ICP. Correct required movement and FragMove.
MovementCompletedReturnMode = i;
i = i + 1;
%% Finished = Robot finished moving.
Finished = i;
i = i + 1;
%%

addpath('../RoombaControl');
addpath('../ros4mat/matlab');
IP = '127.0.0.1';
maxKinectSight = 4;%m
minKinectSight = 0.8;%m
explorationTime = 1800; %seconds
explorationStartTime = tic;
isRoutineFinished = false;
state = InitialState;
initSensors;

isExploring = true;

while(~isRoutineFinished)
    switch state
        case{InitialState}
            initSensors;
            continue;
            
        case{MapSurroundings} %TODO depends on which state we are
            
            state = DecideAndOrient;
            continue;
         
        case{DecideAndOrient} %TODO depends on which state we are
            state = FragmentMove;
            continue;    
            
        case{DropBreadCrumbExplore} % Based on the last known current position, drop a breadcrumb.
            state = MapSurroundings;
            continue;

        case{MovementCompletedExplore}
            if(isExplorationMode)
                state = DropBreadCrumbExplore;
                continue;
            else
                state = InitReturnToBaseMode;
                continue;
            end
            
            
        case{InitReturnToBaseMode}
            isExploring = false;
            continue;
            
        case{MovementCompletedReturnMode}
            continue;
            
        case{Finished}
            isRoutineFinished = true;
            continue;
            
        case{FragmentMove}
            state = FragmentICP;
            continue;
            
        case{FragmentICP} %TODO depends on which state we are
            movementCompleted = false; % TODO : Do a check for remaining movement.
            if(movementCompleted)
                if(isExploring)
                    state = MovementCompletedExplore;
                else
                    state = MovementCompletedReturnMode;
                end
            else
                state = FragmentMove;
            end
            continue;
    end
    
    
end

ros4mat('close');
